#ifndef HNURM_UAV
#define HNURM_UAV

//! ROS
#include <ros/ros.h>
#include <tf/tf.h>

//! ROS standard msgs
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PointStamped.h>
// #include <sensor_msgs/Imu.h>
// #include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/TimeReference.h>
#include <sensor_msgs/BatteryState.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>

// DJI SDK includes
#include <dji_sdk/DroneTaskControl.h>
#include <dji_sdk/SDKControlAuthority.h>
#include <dji_sdk/QueryDroneVersion.h>
#include <dji_sdk/SetLocalPosRef.h>
#include "dji_sdk/dji_sdk.h"

#include <atomic>
enum class RemoteControlState {
    Disconnected,  // 断联
    Centered,      // 回中
    Controlling    // 操控
};

struct current_pos
{
    double pos_x ;
    double pos_y ;
    double pos_z ;
    double pos_yaw ;
};

class HNURM_UAV_Node
    {
        public:
            HNURM_UAV_Node(ros::NodeHandle& nh, ros::NodeHandle& nh_private);
            ~HNURM_UAV_Node();

            float current_height;
            // PD 控制参数
            float Kp_x = 2.0;
            float Kp_y = 2.0;
            float Kd_x = 0.5;
            float Kd_y = 0.5;
            float Kp_z = 2.0;
            float Kd_z = 0.5;
            float prev_error_x = 0.0;
            float prev_error_y = 0.0;
            float prev_error_z = 0.0;
            float dt = 0.01;

            const float deg2rad = M_PI/180.0;
            const float rad2deg = 180.0/M_PI;
            uint8_t flight_status = 255;
            uint8_t display_mode  = 255;
            // geometry_msgs::Quaternion current_atti;
            double current_atti = 0.0;   //transformed to eulerangle

            RemoteControlState RC_state;
            current_pos current;
            current_pos hover;
            sensor_msgs::Joy RC_control;

            ros::Publisher ctrlPosYawPub;
            ros::Timer timer_;
            ros::Subscriber localPosition_sub;
            ros::Subscriber RemoteControler_sub;
            ros::Subscriber height_sub;

            ros::Subscriber flightStatusSub;
            ros::Subscriber displayModeSub;
            ros::Subscriber attitudeSub;

            ros::ServiceClient set_local_pos_reference;
            ros::ServiceClient sdk_ctrl_authority_service;
            ros::ServiceClient drone_task_service;
            ros::ServiceClient query_version_service;

            bool obtain_control_result;
            bool takeoff_result = false;
            std::atomic<bool> RCcontrol{true};


        private:
            // bool initSubscriber(ros::NodeHandle& nh);
            // bool initPublisher(ros::NodeHandle& nh);
            // bool initActions(ros::NodeHandle& nh);    // 定义一些action用于控制?

            void Remote_controller_callback(const sensor_msgs::Joy::ConstPtr& msg);
            // void Barometer_callback();   //似乎读不到气压计 但是可直接读取起飞高度
            void local_position_callback(const nav_msgs::Odometry::ConstPtr& msg);
            void local_position_ctrl(double &xCmd, double &yCmd,double &zCmd,double &yawCmd);
            void timer_callback(const ros::TimerEvent&);
            void height_sub_callback(const std_msgs::Float32::ConstPtr& msg);
            void flight_status_callback(const std_msgs::UInt8::ConstPtr& msg);
            void display_mode_callback(const std_msgs::UInt8::ConstPtr& msg);
            void attitude_callback(const geometry_msgs::QuaternionStamped::ConstPtr& msg);

            bool obtain_control();
            bool release_control();
            bool set_local_position();
            bool is_M100();
            bool monitoredTakeoff();

            void monitorControlPermissions();




    };



#endif
